#include "MyBayer.h"

int getPosition(int x, int y, int width)
{
	return (x * width + y);
}

int getPositionColor(int x, int y, int offset, int width, int depth)
{
	return ((x * width + y) * depth + offset);
}

void getGRBGForG(int x, int y, int width, int height, double* r, double* b, const u8* input) {
	if ((x & 1) == 1) {
		float bleft = input[getPosition(x, y - 1, width)];
		float rup = input[getPosition(x - 1, y, width)];

		*r = (rup + (x == height - 1 ? rup : input[getPosition(x + 1, y, width)])) / 2;
		*b = (bleft + (y == width - 1 ? bleft : input[getPosition(x, y + 1, width)])) / 2;
	}
	else {
		float rreft = input[getPosition(x, y + 1, width)];
		float bdown = input[getPosition(x + 1, y, width)];
		*r = (rreft + (y == 0 ? rreft : input[getPosition(x, y - 1, width)])) / 2;
		*b = (bdown + (x == 0 ? bdown : input[getPosition(x -  1, y, width)])) / 2;
	}
}
void getGRBGForR(int x, int y, int widht, int height, double* g, double* b, const u8* input) {
	bool firstrow = x == 0;
	bool lastcol = y == widht - 1;
	float gl = input[getPosition(x, y - 1, widht)];
	float gd = input[getPosition(x + 1, y, widht)];
	float gu = firstrow ? gd : input[getPosition(x - 1, y, widht)];
	float gr = lastcol ? gl : input[getPosition(x, y + 1, widht)];
	float bld = input[getPosition(x + 1, y - 1, widht)];
	float blu = firstrow ? bld : input[getPosition(x - 1, y - 1, widht)];
	float bru = (firstrow || lastcol) ? bld : input[getPosition(x - 1, y + 1, widht)];
	float brd = lastcol ? bld : input[getPosition(x + 1, y + 1, widht)];
	*g = (gl + gr + gd + gu) / 4;
	*b = (bld + brd + blu + bru) / 4;
}
void getGRBGForB(int x, int y, int widht, int height, double* g, double* r, const u8* input) {
	bool firstcol = y == 0;
	bool lastrow = x == height - 1;
	float gu = input[getPosition(x - 1, y, widht)];
	float gr = input[getPosition(x, y + 1, widht)];
	float gl = firstcol ? gr : input[getPosition(x, y - 1, widht)];
	float gd = lastrow ? gu : input[getPosition(x + 1, y, widht)];
	float rru = input[getPosition(x - 1, y + 1, widht)];
	float rlu = firstcol ? rru : input[getPosition(x - 1, y - 1, widht)];
	float rld = (lastrow || firstcol) ? rlu : input[getPosition(x + 1, y - 1, widht)];
	float rrd = lastrow ? rru : input[getPosition(x + 1, y + 1, widht)];

	*g = (gu + gl + gd + gr) / 4;
	*r = (rru + rlu + rld + rrd) / 4;
}


//BGGR
void getBGGRForG(int x, int y, int width, int height, float* r, float* b, const u8* input) {
	if ((x & 1) == 1) {
		float blue_up = input[getPosition(x - 1, y, width)];
		float red_right = input[getPosition(x, y + 1, width)];
		*r = (red_right + (y == 0 ? red_right : input[getPosition(x, y - 1, width)])) / 2;
		*b = (blue_up + (x == height - 1 ? blue_up : input[getPosition(x + 1, y, width)])) / 2;
	}
	else {
		float blue_left = input[getPosition(x, y - 1, width)];
		float red_down = input[getPosition(x + 1, y, width)];

		*r = (red_down + (x == 0 ? red_down : input[getPosition(x - 1, y, width)])) / 2;
		*b = (blue_left + (y == width - 1 ? blue_left : input[getPosition(x, y + 1, width)])) / 2;
	}
}
void getBGGRForR(int x, int y, int widht, int height, float* g, float* b, const u8* input) {
	bool last_row = x == height - 1;
	bool lastcol = y == widht - 1;
	float green_left = input[getPosition(x, y - 1, widht)];
	float green_up = input[getPosition(x - 1, y, widht)];
	float green_down = last_row ? green_up : input[getPosition(x + 1, y, widht)];
	float green_right = lastcol ? green_left : input[getPosition(x, y + 1, widht)];
	float blue_left_up = input[getPosition(x - 1, y - 1, widht)];
	float blue_left_down = last_row ? blue_left_up : input[getPosition(x + 1, y - 1, widht)];
	float blue_right_down = (last_row || lastcol) ? blue_left_up : input[getPosition(x + 1, y + 1, widht)];
	float blue_right_up = lastcol ? blue_left_up : input[getPosition(x - 1, y + 1, widht)];
	*g = (green_down + green_right + green_up + green_left) / 4;
	*b = (blue_left_up + blue_left_down + blue_right_down + blue_right_up) / 4;
}
void getBGGRForB(int x, int y, int widht, int height, float* g, float* r, const u8* input) {
	bool first_col = y == 0;
	bool first_row = x == 0;
	float green_down = input[getPosition(x + 1, y, widht)];
	float green_right = input[getPosition(x, y + 1, widht)];
	float green_left = first_col ? green_right : input[getPosition(x, y - 1, widht)];
	float green_up = first_row ? green_down : input[getPosition(x - 1, y, widht)];
	float red_right_down = input[getPosition(x + 1, y + 1, widht)];
	float red_right_up = first_row ? red_right_down : input[getPosition(x - 1, y + 1, widht)];
	float red_left_up = (first_col || first_row) ? red_right_down : input[getPosition(x - 1, y - 1, widht)];
	float rrd = first_col ? red_right_down : input[getPosition(x + 1, y - 1, widht)];

	*g = (green_up + green_left + green_down + green_right) / 4;
	*r = (red_right_up + red_right_down + red_right_down + red_left_up) / 4;
}


//RGGB 和 BGGR 互相交换 BR就可以了
void getRGGBForG(int x, int y, int width, int height, float* r, float* b, const u8* input) {
	if ((x & 1) == 1) {
		float blue_up = input[getPosition(x - 1, y, width)];
		float red_right = input[getPosition(x, y + 1, width)];
		*b = (red_right + (y == 0 ? red_right : input[getPosition(x, y - 1, width)])) / 2;
		*r = (blue_up + (x == height - 1 ? blue_up : input[getPosition(x + 1, y, width)])) / 2;
	}
	else {
		float blue_left = input[getPosition(x, y - 1, width)];
		float red_down = input[getPosition(x + 1, y, width)];

		*b = (red_down + (x == 0 ? red_down : input[getPosition(x - 1, y, width)])) / 2;
		*r = (blue_left + (y == width - 1 ? blue_left : input[getPosition(x, y + 1, width)])) / 2;
	}
}
void getRGGBForR(int x, int y, int widht, int height, float* g, float* b, const u8* input) {

	bool first_col = y == 0;
	bool first_row = x == 0;
	float green_down = input[getPosition(x + 1, y, widht)];
	float green_right = input[getPosition(x, y + 1, widht)];
	float green_left = first_col ? green_right : input[getPosition(x, y - 1, widht)];
	float green_up = first_row ? green_down : input[getPosition(x - 1, y, widht)];
	float red_right_down = input[getPosition(x + 1, y + 1, widht)];
	float red_right_up = first_row ? red_right_down : input[getPosition(x - 1, y + 1, widht)];
	float red_left_up = (first_col || first_row) ? red_right_down : input[getPosition(x - 1, y - 1, widht)];
	float rrd = first_col ? red_right_down : input[getPosition(x + 1, y - 1, widht)];

	*g = (green_up + green_left + green_down + green_right) / 4;
	*b = (red_right_up + red_right_down + red_right_down + red_left_up) / 4;
}
void getRGGBForB(int x, int y, int widht, int height, float* g, float* r, const u8* input) {
	bool last_row = x == height - 1;
	bool lastcol = y == widht - 1;
	float green_left = input[getPosition(x, y - 1, widht)];
	float green_up = input[getPosition(x - 1, y, widht)];
	float green_down = last_row ? green_up : input[getPosition(x + 1, y, widht)];
	float green_right = lastcol ? green_left : input[getPosition(x, y + 1, widht)];
	float blue_left_up = input[getPosition(x - 1, y - 1, widht)];
	float blue_left_down = last_row ? blue_left_up : input[getPosition(x + 1, y - 1, widht)];
	float blue_right_down = (last_row || lastcol) ? blue_left_up : input[getPosition(x + 1, y + 1, widht)];
	float blue_right_up = lastcol ? blue_left_up : input[getPosition(x - 1, y + 1, widht)];
	*g = (green_down + green_right + green_up + green_left) / 4;
	*r = (blue_left_up + blue_left_down + blue_right_down + blue_right_up) / 4;
}




void SetOutPutRGB(u8 r, u8 g, u8 b, int x, int y, u8* output, int width)
{
	output[getPositionColor(x, y, RED, width, 3)] = r;
	output[getPositionColor(x, y, GREEN, width, 3)] = g;
	output[getPositionColor(x, y, BLUE, width, 3)] = b;
}

bool BayerConvertRGGB(u8* bayer, u8* rgb, int width, int height, float light)
{
#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x = 0, y = 0;
		x = i / width;
		y = i % width;
		int value = (x & 1) == 1 ? ((y & 1) == 1 ? BLUE : GREEN) : ((y & 1) == 1 ? GREEN : RED);
		float r = 0, g = 0, b = 0;

		switch (value)
		{
		case RED:
			r = bayer[i];
			getRGGBForR(x, y, width, height, &g, &b, bayer);
			break;
		case GREEN:
			g = bayer[i];
			getRGGBForG(x, y, width, height, &r, &b, bayer);
			break;
		case BLUE:
			b = bayer[i];
			getRGGBForB(x, y, width, height, &g, &r, bayer);
			break;
		default:
			break;
		}
		r *= light;
		g *= light;
		b *= light;
		if (r > 255)r = 255;
		if (g > 255)g = 255;
		if (b > 255)b = 255;

		SetOutPutRGB((u8)r, (u8)g, (u8)b, x, y, rgb, width);
	}
#
	return true;
}

bool BayerConvertGRBG(u8* bayer, u8* rgb, int width, int height, float light)
{
#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x = 0, y = 0;
		x = i / width;
		y = i % width;
		int value = (x & 1) == 1 ? ((y & 1) == 1 ? GREEN : BLUE) : ((y & 1) == 1 ? RED : GREEN);
		double r = 0, g = 0, b = 0;

		switch (value)
		{
		case RED:
			r = bayer[i];
			getGRBGForR(x, y, width, height, &g, &b, bayer);
			break;
		case GREEN:
			g = bayer[i];
			getGRBGForG(x, y, width, height, &r, &b, bayer);
			break;
		case BLUE:
			b = bayer[i];
			getGRBGForB(x, y, width, height, &g, &r, bayer);
			break;
		default:
			break;
		}
		r *= light;
		g *= light;
		b *= light;
		if (r > 255)r = 255;
		if (g > 255)g = 255;
		if (b > 255)b = 255;

		SetOutPutRGB((u8)r, (u8)g, (u8)b, x, y, rgb, width);
	}
#
	return true;
}
//BGGRת��
bool BayerConvertBGGR(u8* bayer, u8* rgb, int width, int height, float light)
{
#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x = 0, y = 0;
		x = i / width;
		y = i % width;
		int value = (x & 1) == 1 ? ((y & 1) == 1 ? RED : GREEN) : ((y & 1) == 1 ? GREEN : BLUE);
		float r = 0, g = 0, b = 0;

		switch (value)
		{
		case RED:
			r = bayer[i];
			getBGGRForR(x, y, width, height, &g, &b, bayer);
			break;
		case GREEN:
			g = bayer[i];
			getBGGRForG(x, y, width, height, &r, &b, bayer);
			break;
		case BLUE:
			b = bayer[i];
			getBGGRForB(x, y, width, height, &g, &r, bayer);
			break;
		default:
			break;
		}
		r *= light;
		g *= light;
		b *= light;
		if (r > 255)r = 255;
		if (g > 255)g = 255;
		if (b > 255)b = 255;

		SetOutPutRGB((u8)r, (u8)g, (u8)b, x, y, rgb, width);
	}
#
	return true;
}

bool BayerConvertGRBG_Copy(u8* bayer, u8* rgb, int width, int height, float light)
{
#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x, y, col, row;
		float r, g, b;

		y = i % width;
		x = i / width;
		col = y - y % 2;
		row = (x & 1) == 1 ? x - 1 : x;
		r = (float)bayer[getPosition(row, col + 1, width)] * light;
		//g = (bayer[getPosition(row, col, width)] + bayer[getPosition(row + 1, col + 1, width)]) / 2;
		g = (float)bayer[getPosition(row, col, width)] * light;
		b = (float)bayer[getPosition(row + 1, col, width)] * light;

		if (r > 255)r = 255;
		if (g > 255)g = 255;
		if (b > 255)b = 255;
		
		SetOutPutRGB((u8)r, (u8)g, (u8)b, x, y, rgb, width);
	}
	return true;
}
//CPUת��GRBG��RGB565ͼ��
bool BayerConvertGRBG_RGB565(u8* bayer, u16* rgb565, int width, int height)
{
	u16 first = 0xf800;
	u16 second = 0x07e0;
	u16 third = 0x001f;

	u16 max_red_blue = 32;
	u16 max_green = 64;
#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x, y, col, row;
		//u16 value;

		y = i % width;
		x = i / width;
		col = y - (y & 1);
		row = (x & 1) == 1 ? x - 1 : x;
		
		u16 blue = (u16)bayer[getPosition(row + 1, col, width)];
		//blue = blue * gain;
		//blue = blue > max_red_blue ? max_red_blue : blue;

		u16 green = (u16)bayer[getPosition(row, col, width)];
		//green = green * gain;
		//green = green > max_green ? max_green : green;

		u16 red = bayer[getPosition(row, col + 1, width)];
		//red = red * gain;
		//red = red > max_red_blue ? max_red_blue : red;

		blue = (blue >> 3 << 11 & first);
		green = (green >> 2 << 5 & second);

		red = red >> 3 & third;


		//要想要显示是RGB  内存中存放的 必须是 BGR
		rgb565[i] = blue | green | red;
	}
#
	return true;
}

bool BayerConvertBGGR_RGB565(u8* bayer, u16* rgb565, int width, int height)
{
	u16 first = 0xf800;
	u16 second = 0x07e0;
	u16 third = 0x001f;

	u16 max_red_blue = 32;
	u16 max_green = 64;

#pragma omp parallel for
	for (int i = 0; i < width * height; i++)
	{
		int x, y, col, row;
		//u16 value;

		y = i % width;
		x = i / width;
		col = y - (y & 1);
		row = (x & 1) == 1 ? x - 1 : x;


		u16 blue = (u16)bayer[getPosition(row, col, width)];
		//blue = blue * gain;
		//blue = blue > max_red_blue ? max_red_blue : blue;

		u16 green = (u16)bayer[getPosition(row, col + 1, width)];
		//green = green * gain;
		//green = green > max_green ? max_green : green;

		u16 red = bayer[getPosition(row + 1, col + 1, width)];
		//red = red * gain;
		//red = red > max_red_blue ? max_red_blue : red;

		blue = (blue >> 3 << 11 & first);
		green = (green >> 2 << 5 & second);
		red = red >> 3 & third;


		//要想要显示是RGB  内存中存放的 必须是 BGR
		rgb565[i] = blue | green | red;

	}
#
	return true;
}


bool BayerConvertGBRG(u8* bayer, u8* rgb, int width, int height)
{
	return false;
}
bool BayerConverter(u8* bayer, u8* rgb, int width, int height, int bayerTile, float light)
{
	switch (bayerTile)
	{
	case RGGB_CPU_O:
		return BayerConvertRGGB(bayer, rgb, width, height, light);
	case GRBG_CPU_O:
		return BayerConvertGRBG(bayer, rgb, width, height, light);
	case BGGR_CPU_O:
		return BayerConvertBGGR(bayer, rgb, width, height, light);
	case GRBG_CPU_C:
		return BayerConvertGRBG_Copy(bayer, rgb, width, height, light);
	case GRBG_CPU_RGB565:
		return BayerConvertGRBG_RGB565(bayer, (u16*)rgb, width, height);
	case BGGR_CPU_RGB565:
		return BayerConvertBGGR_RGB565(bayer, (u16*)rgb, width, height);
	case GBRG:
		return BayerConvertGBRG(bayer, rgb, width, height);
	default:
		return false;
	}
	return true;
}

void ImageIncreaseBrightness(u8* data, int w, int h, int deepth, float light)
{
#pragma omp parallel for
	for (int i = 0; i < w * h * deepth; i++)
	{
		float v = data[i];
		v *= light;
		if (v > 255) v = 255;
		data[i] = (u8)v;
	}
}



// 灰度图转三通道图
void ImageMono8ToGrayConverter(u8* source, u8* desc, size_t len) {
#pragma omp parallel for
	for (int i = 0; i < len; i++)
	{
		desc[i * 3] = source[i];
		desc[i * 3 + 1] = source[i];
		desc[i * 3 + 2] = source[i];
	}
}
